/**
 * @file
 * @brief Defines the canbus manager.
 */

#include "canbus/manager.h"

using namespace canbus;

int main(int argc, char** argv) {

  ros::init(argc, argv, "ws_canbus");
  CanbusManager manager;
  manager.Init();
  manager.Start();

  InterfaceData* return_data;
  VehicleCmd vcmd;
  VehicleCtrl vctrl;
  SpreaderCtrl sctrl;

  // std::string type = "vehicle_sensor";=
  // ros::Duration t(0.001);
  while (ros::ok()) {

    manager.WriteVehicleCmd(&vcmd);
    manager.WriteVehicleCtrl(&vctrl);
    manager.WriteSpreaderCmd(&sctrl);

    // if (type == "vehicle_status") {
      return_data = manager.ReadVehicleStatus();
    // } else if (type == "vehicle_sensor") {
      return_data = manager.ReadVehicleSensor();
    // } else if (type == "vehicle_state") {
      return_data = manager.ReadVehicleState();
    // } else if (type == "spreader_state") {
      return_data = manager.ReadSpreaderState();
    // } else if (type == "wheel_encoder") {
      return_data = manager.ReadWheelEncoder();
    // }

    std::this_thread::sleep_for(std::chrono::milliseconds(100));
  }

  return 0;
}